﻿#include "Matrix.h"
#include "Constant.h"
#include "CoordSys.h"
#include "TimeSys.h"
#include "ReadN.h"
#include "ReadO.h"
#include "GNSSSatPos.h"
#include "IonoCorr.h"
#include "TropCorr.h"
#include "literal.h"
#include <iostream>
#include <iomanip>
#include <vector>
#include <cmath>
using namespace std;

// 单点定位数据处理全过程说明：
/*
1. 数据准备阶段
   - 读取RINEX观测文件(O文件)：获取接收机观测值(伪距、载波相位等)
   - 读取RINEX导航文件(N文件)：获取卫星星历和时钟参数
   - 初始化时间系统、坐标系统和常量参数

2. 迭代处理每个观测历元
   for 每个观测历元:
    3. 获取当前历元的所有卫星观测数据
    4. 对每颗可见卫星:
        a. 从导航数据中获取对应卫星的星历
        b. 计算卫星位置和卫星钟差
        c. 计算信号传播时间
        d. 计算地球自转校正(Sagnac效应)
        e. 应用电离层和对流层延迟改正
        f. 计算几何距离和观测残差

    5. 建立观测方程
        - 设计矩阵A (几何距离的方向余弦)
        - 观测向量L (伪距残差)

    6. 最小二乘法求解
        - 计算位置增量: ΔX = (AᵀA)⁻¹AᵀL
        - 更新接收机位置和接收机钟差

    7. 检查收敛条件
        - 位置增量小于阈值(如1e-4米)
        - 达到最大迭代次数(如10次)

    8. 输出定位结果
        - 大地坐标(B, L, H)
        - 接收机钟差
        - 精度因子(PDOP)

3. 结束处理
   - 保存结果
   - 释放资源
*/
int main() {
    // 初始化时间系统(使用北斗时间)
    BDSTime mBDST;
    TimeSys* pTimeSys = &mBDST;

    // 初始化坐标系统(使用北斗坐标系)
    BDSConsts bdsConsts;
    RefEllip ellip = bdsConsts.get_refEllip();
    BDCS bdcs(ellip);
    CoordSys* pCoordSys = &bdcs;

    // 初始化常数
    GNSSconsts* pGnssConst = &bdsConsts;
    double CLIGHT = pGnssConst->get_SpeedOfLight();
    double R2D = R2D1;  // 180.0 / PI 的近似值

    // 读取广播星历文件(N文件)
    string nFileName = "C:/1234560660442.17N";
    ReadN mReadN(nFileName);
    if (!mReadN.parseFile()) {
        cerr << "解析N文件失败!" << endl;
        return -1;
    }
    cout << "N文件解析成功" << endl;

    // 读取观测值文件(O文件)
    string oFileName = "C:/1234560660442.17O";
    ReadO mReadO(oFileName);
    if (!mReadO.parse_file()) {
        cerr << "解析O文件失败!" << endl;
        return -1;
    }
    cout << "O文件解析成功" << endl;

    // 获取电离层改正器
    IonoCorr mIonCor(pGnssConst, &mReadN.get_head());

    // 获取对流层改正器
    TropCorr mTropCor(pGnssConst);

    // 获取卫星位置计算器
    ReadN* pReadN = &mReadN;
    BDSSatPos mBDSSatPos(pTimeSys, pReadN);
    GNSSSatPos* pSatPos = &mBDSSatPos;

    // 获取观测数据
    vector<obsEpoch>* obsData = mReadO.get_obs();
    if (obsData->empty()) {
        cerr << "没有观测数据!" << endl;
        return -1;
    }

    // 使用O文件头中的近似位置作为初始值
    XYZ recvPos = mReadO.get_AppxPos();
    double recvClockBias = 0.0;  // 接收机钟差初始值

    // 输出初始位置
    cout << "\n初始接收机位置(ECEF):" << endl;
    cout << fixed << setprecision(3);
    cout << "  X: " << recvPos.x << " 米" << endl;
    cout << "  Y: " << recvPos.y << " 米" << endl;
    cout << "  Z: " << recvPos.z << " 米" << endl;

    // 获取第一个历元的观测数据
    obsEpoch epoch = obsData->at(0);
    BaseT obsTime = pTimeSys->str2baseT(epoch.time);
    cout << "观测时间(秒): " << obsTime.sec << "." << obsTime.sesec << endl;

    // 单点定位迭代
    const int MAX_ITER = 10;
    const double CONV_THRESH = 0.01;  // 收敛阈值0.01米
    Matrix dx(4, 1);  // 位置和钟差改正量

    for (int iter = 0; iter < MAX_ITER; iter++) {
        int numSats = epoch.listObsSat.size();
        cout << "\n=== 迭代 " << iter + 1 << " ===" << endl;
        cout << "当前历元卫星总数: " << numSats << endl;

        if (numSats < 4) {
            cerr << "可见卫星不足(" << numSats << "颗)，无法定位!" << endl;
            break;
        }

        // 设计矩阵A: numSats x 4
        Matrix A(numSats, 4);

        // 残差向量L: numSats x 1
        Matrix L(numSats, 1);

        // 权矩阵P: numSats x numSats (对角阵)
        Matrix P(numSats, numSats);

        int validSatCount = 0;  // 有效卫星计数

        for (int i = 0; i < numSats; i++) {
            obsSat satObs = epoch.listObsSat[i];
            string prn = satObs.satID;

            // 调试输出
            cout << "\n处理卫星: " << prn;

            // 只处理北斗卫星(C开头)
            if (prn[0] != 'C') {
                cout << " - 跳过(非北斗卫星)" << endl;
                continue;
            }

            // 获取伪距观测值(第一个频率)
            if (satObs.values.empty()) {
                cout << " - 跳过(无观测值)" << endl;
                continue;
            }
            double pseudoRange = satObs.values[0];
            cout << ", 伪距: " << fixed << setprecision(3) << pseudoRange << " m";

            // 获取卫星位置和钟差
            XYZ satPos = pSatPos->get_satPos(prn, obsTime);
            cout << ", 卫星位置: ("
                << satPos.x << ", " << satPos.y << ", " << satPos.z << ")";

            // 检查卫星位置是否有效
            if (satPos.x == 0 && satPos.y == 0 && satPos.z == 0) {
                cout << " - 卫星位置计算失败!" << endl;
                continue;
            }

            // 计算几何距离
            double dx_val = satPos.x - recvPos.x;
            double dy_val = satPos.y - recvPos.y;
            double dz_val = satPos.z - recvPos.z;
            double geoDist = sqrt(dx_val * dx_val + dy_val * dy_val + dz_val * dz_val);
            cout << ", 几何距离: " << geoDist << " m";

            // 计算接收机到卫星的方位角和高度角
            Azel azel = pCoordSys->get_azel(recvPos, satPos);
            double elev = azel.ele * R2D;  // 转换为度
            cout << ", 高度角: " << elev << "°";

            // 高度角过低则跳过(降低到0度)
            if (elev < 0.0) {
                cout << " - 跳过(高度角过低)" << endl;
                continue;
            }

            // 计算电离层延迟(单位：米)
            BLH blh = pCoordSys->XYZ2BLH(recvPos);
            SOW sow = pTimeSys->calend2sow(pTimeSys->baseT2calend(obsTime));
            double ionoDelay = mIonCor.get_ionoCorr(blh, azel, sow);
            cout << ", 电离层延迟: " << ionoDelay << " m";

            // 计算对流层延迟(单位：米)
            double tropDelay = mTropCor.get_tropCorr(blh, azel.ele);
            cout << ", 对流层延迟: " << tropDelay << " m";

            // 计算卫星钟差(单位：米)
            BroadEphemeris* eph = mReadN.get_Ephemeris(prn, obsTime);
            double satClockBias = 0.0;
            if (eph) {
                // 使用星历中的时间信息构建toc
                CalendT toc_calend = {
                    eph->year, eph->month, eph->day,
                    eph->hour, eph->min,
                    static_cast<int>(eph->sec),
                    eph->sec - static_cast<int>(eph->sec)
                };
                BaseT toc = pTimeSys->calend2baseT(toc_calend);

                // 计算时间差 tk = obsTime - toc
                double t_obs = obsTime.sec + obsTime.sesec;
                double t_toc = toc.sec + toc.sesec;
                double tk = t_obs - t_toc;

                // 卫星钟差模型：a0 + a1*tk + a2*tk^2
                satClockBias = eph->clockBias + eph->clockDrift * tk +
                    eph->clockDriftRate * tk * tk;
                satClockBias *= CLIGHT;  // 转换为距离
                cout << ", 卫星钟差: " << satClockBias << " m";
            }
            else {
                cout << " - 未找到星历!";
                continue;
            }

            // 计算改正后的伪距观测值
            double correctedPR = pseudoRange + satClockBias - ionoDelay - tropDelay;

            // 计算观测残差
            double residual = correctedPR - (geoDist + CLIGHT * recvClockBias);
            cout << ", 残差: " << residual << " m";

            // 计算方向余弦
            double ax = dx_val / geoDist;
            double ay = dy_val / geoDist;
            double az = dz_val / geoDist;

            // 填充设计矩阵A
            A[validSatCount][0] = ax;
            A[validSatCount][1] = ay;
            A[validSatCount][2] = az;
            A[validSatCount][3] = 1.0;  // 钟差系数

            // 填充残差向量L
            L[validSatCount][0] = residual;

            // 设置权值(按高度角定权)
            double weight = sin(azel.ele);  // 高度角(弧度)
            P[validSatCount][validSatCount] = weight * weight;

            cout << " - 已加入定位解算" << endl;
            validSatCount++;
        }

        cout << "有效卫星数量: " << validSatCount << endl;

        // 实际有效卫星数
        if (validSatCount < 4) {
            cerr << "有效卫星不足(" << validSatCount << "颗)，无法解算!" << endl;
            break;
        }

        // 裁剪矩阵到实际有效大小
        Matrix A_valid(validSatCount, 4);
        Matrix L_valid(validSatCount, 1);
        Matrix P_valid(validSatCount, validSatCount);

        for (int i = 0; i < validSatCount; i++) {
            for (int j = 0; j < 4; j++) {
                A_valid[i][j] = A[i][j];
            }
            L_valid[i][0] = L[i][0];
            for (int j = 0; j < validSatCount; j++) {
                P_valid[i][j] = (i == j) ? P[i][i] : 0.0;
            }
        }

        // 最小二乘解算: dx = (A^T*P*A)^(-1) * A^T*P*L
        Matrix AT = A_valid.transpose();
        Matrix ATP = AT * P_valid;
        Matrix ATPA = ATP * A_valid;

        if (!ATPA.invert()) {
            cerr << "矩阵求逆失败!" << endl;
            break;
        }

        Matrix ATPL = ATP * L_valid;
        dx = ATPA * ATPL;

        // 更新接收机位置和钟差
        recvPos.x += dx[0][0];
        recvPos.y += dx[1][0];
        recvPos.z += dx[2][0];
        recvClockBias += dx[3][0] / CLIGHT;

        // 输出更新后的位置
        cout << "更新后接收机位置:" << endl;
        cout << "  X: " << recvPos.x << " 米" << endl;
        cout << "  Y: " << recvPos.y << " 米" << endl;
        cout << "  Z: " << recvPos.z << " 米" << endl;
        cout << "接收机钟差: " << recvClockBias << " 秒" << endl;

        // 检查收敛条件
        double norm = sqrt(dx[0][0] * dx[0][0] + dx[1][0] * dx[1][0] + dx[2][0] * dx[2][0]);
        cout << "位置改正量 = " << norm << " 米" << endl;

        if (norm < CONV_THRESH) {
            cout << "收敛条件满足!" << endl;
            break;
        }
    }

    // 输出定位结果
    cout << "\n=== 单点定位结果 ===" << endl;
    cout << fixed << setprecision(3);
    cout << "接收机位置(ECEF坐标系):" << endl;
    cout << "  X: " << recvPos.x << " 米" << endl;
    cout << "  Y: " << recvPos.y << " 米" << endl;
    cout << "  Z: " << recvPos.z << " 米" << endl;

    // 计算并输出大地坐标
    BLH blh = pCoordSys->XYZ2BLH(recvPos);
    cout << "\n接收机位置(大地坐标系):" << endl;
    cout << fixed << setprecision(9);
    cout << "  纬度(B):  " << blh.B << " 度" << endl;
    cout << "  经度(L): " << blh.L << " 度" << endl;
    cout << fixed << setprecision(3);
    cout << "  高程(H):    " << blh.H << " 米" << endl;

    // 输出接收机钟差
    cout << "\n接收机钟差: " << scientific << recvClockBias << " 秒" << endl;

    // 输出改正数
    cout << "\n改正数:" << endl;
    cout << fixed << setprecision(3);
    cout << "  X方向改正: " << dx[0][0] << " 米" << endl;
    cout << "  Y方向改正: " << dx[1][0] << " 米" << endl;
    cout << "  Z方向改正: " << dx[2][0] << " 米" << endl;
    cout << "  钟差改正: " << dx[3][0] << " 米" << endl;

    return 0;
}